package oneD.fem.element.nonlinear;

import oneD.fem.Node;
import oneD.fem.algorithm.MyLinalg;
import oneD.fem.element.linear.Truss;
import inf.jlinalg.Array2DMatrix;
import inf.jlinalg.ArrayVector;
import inf.jlinalg.BLAM;
import inf.jlinalg.IMatrix;
import inf.jlinalg.IVector;

public class PrestressTruss extends Truss {
	protected double preStress_;

	private IMatrix A_;

	public PrestressTruss(double rho, double e, double a, double preS,
			Node n1, Node n2) {
		super(rho, e, a, n1, n2);
		this.preStress_ = preS;
		// initialize value for A
		A_ = new Array2DMatrix(6, 6);
		for (int i = 0; i < 6; i++)
			A_.set(i, i, 1);
		for (int i = 0; i < 3; i++) {
			A_.set(i + 3, i, -1);
			A_.set(i, i + 3, -1);
		}
	}

	private IMatrix getA() {
		return A_;
	}

	protected double computeE11(IVector ue) {
		IVector Xe = this.getXe();
		// BLAM.add(0.5, ue, Xe); // Xe <- Xe+0.5ue
		MyLinalg.add(0.5, ue, Xe);
		IVector Aue = (IVector) ue.clone(); // just initialize the value, it can
											// be arbitrary
		BLAM.multiply(1 / Math.pow(this.getLength(), 2), BLAM.NO_TRANSPOSE,
				this.getA(), ue, 0, Aue); // Aue <- A*ue*1/L^2
		return BLAM.dot(Xe, Aue);
	}

	protected double computeStress(IVector ue) {
		double stress = this.eModulus_ * this.computeE11(ue) + this.preStress_;
		return stress;
	}

	@Override
	public IVector computeInternalForcesVector() {
		IVector Xe = this.getXe();
		// System.out.println("Element Xe: " + MatrixFormat.format(Xe));
		IVector ue = this.getUe();
		// System.out.println("Element ue before: " + MatrixFormat.format(ue));
		double stress = this.computeStress(ue);
		// BLAM.add(1, ue, Xe); // xe <- Xe+ue
		MyLinalg.add(1, ue, Xe);
		// System.out.println("Element ue after: " + MatrixFormat.format(ue));
		// System.out.println("Element xe before: " + MatrixFormat.format(Xe));
		IVector re = new ArrayVector(6);
		double reCoeff = this.sectionArea_ / this.getLength() * stress;
		// System.out.println("Element stress = " + stress);
		// System.out.println("Element A/L*stress = " + reCoeff);
		BLAM.multiply(reCoeff, BLAM.NO_TRANSPOSE, this.getA(), Xe, 0, re); // result
																			// <-
																			// a/L*A*xe*stress
		// System.out.println("Element " + this.getDOFNumbers().toString()
		// + " internal forces vector:");
		// System.out.println(MatrixFormat.format(re));
		// System.out.println("Element xe after: " + MatrixFormat.format(Xe));
		// System.out.println();
		return re;
	}

	@Override
	public IMatrix computeGeometricalStiffnessMatrix() {
		double stress = this.computeStress(this.getUe());
//		System.out.println("Element stress = " + stress + ", prestress = " + this.preStress_);
		double kegCoeff = this.sectionArea_ / this.getLength() * stress;
		IMatrix keg = (IMatrix) this.getA().clone();
		BLAM.scale(kegCoeff, keg);
		return keg;
	}

	private IMatrix computeMaterialStiffnessMatrix() {
		double kemCoeff = this.eModulus_ * this.sectionArea_
				/ Math.pow(this.getLength(), 3);
		IMatrix X = new Array2DMatrix(6, 6);
		IMatrix temp = new Array2DMatrix(6, 6);
		IMatrix kem = new Array2DMatrix(6, 6);
		BLAM.zero(X);
		IVector xe = this.getXe();
		IVector ue = this.getUe();
		// BLAM.add(1, ue, xe); // xe <- Xe+ue
		MyLinalg.add(1, ue, xe);
		BLAM.multiply(kemCoeff, xe, xe, X);
		BLAM.multiply(1, BLAM.NO_TRANSPOSE, this.getA(), BLAM.NO_TRANSPOSE, X,
				0, temp);
		BLAM.multiply(1, BLAM.NO_TRANSPOSE, temp, BLAM.NO_TRANSPOSE,
				this.getA(), 0, kem);
		return kem;
	}

	@Override
	public IMatrix computeTangentStiffnessMatrix() {
		IMatrix ke = this.computeMaterialStiffnessMatrix();
		/*
		 * IMatrix kge = this.computeGeometricalStiffnessMatrix();
		 * System.out.println("Element " + this.getDOFNumbers().toString() +
		 * " matrix listing"); System.out.println("Material stiffness:");
		 * System.out.print(MatrixFormat.format(ke));
		 * System.out.println("Geometrical stiffness:");
		 * System.out.print(MatrixFormat.format(kge));
		 */
		BLAM.add(1, this.computeGeometricalStiffnessMatrix(), ke);
		return ke;
	}

	@Override
	public double computeForce() {
		return this.sectionArea_ * this.computeStress(this.getUe());
	}

	@Override
	public void print() {
		System.out.print(this.eModulus_ + "\t" + this.sectionArea_ + "\t"
				+ this.getLength() + "\t" + this.preStress_);
	}
	
	@Override
	public void setPreStress(double stress) {
		this.preStress_ = stress;
	}
}
